import time

import serial

from my_camera import Camera
from my_serial import Com
import cv2 as cv
import numpy as np
import threading as td
import glob


def descartes_distance(point1, point2):
    delta_x = abs(point1[0] - point2[0])
    delta_y = abs(point1[1] - point2[1])
    distance = (delta_x ** 2 + delta_y ** 2) ** 0.5
    return distance


def is_array_in_list(array, array_list):
    for a in array_list:
        a = np.array(a)
        if all(array == a):
            return True
    return False


class SortingRobot(object):
    def __init__(self):
        self.camera = Camera(0)
        self.camera.set(cv.CAP_PROP_FRAME_WIDTH, 1280)
        self.camera.set(cv.CAP_PROP_FRAME_HEIGHT, 720)
        self.camera.read()
        self.__work_space = ((0, 0), (300, 250))
        self.__stack_space = ((30, 220), (170, 300))
        try:
            self.com = serial.Serial("COM8", 115200, timeout=5)
        except Exception:
            self.com = None

        self.__show_camera_running = False
        self.showCameraThread = td.Thread()

    def open_com(self):
        self.com = Com("COM8", 115200, timeout=5)
        # self.com = Com("/dev/ttyUSB0", 115200, timeout=5)
        self.com.activate_receive()

    def activate_show_camera(self):
        self.__show_camera_running = True

        def show_camera_loop():
            while self.__show_camera_running:
                ret, frame = self.camera.read()
                time.sleep(0.1)

        self.showCameraThread = td.Thread(target=show_camera_loop)
        self.showCameraThread.start()

    def deactivate_show_camera(self):
        self.__show_camera_running = False
        cv.destroyWindow('camera')

    def calibrate_camera_in_mtx(self):
        images = []
        # image_names = glob.glob('/home/pi/Documents/Sorting/calibrate/*.jpg')
        image_names = glob.glob('./calibrate/*.jpg')
        for name in image_names:
            images.append(cv.imread(name))

        self.camera.calibrate_intrinsic_matrix(images, col=9, row=6)
        print(self.camera.in_mtx)

    def wait_for_ac(self):
        while True:
            if not self.com.recvBuffer.empty():
                temp = self.com.recvBuffer.get()
                if temp == 'Action Complete\n':
                    print(temp)
                    break

    def wait_for_bc(self):
        while True:
            if not self.com.recvBuffer.empty():
                temp = self.com.recvBuffer.get()
                if temp == 'Serial Complete\n':
                    print(temp)
                    break

    def wait_for_cc(self):
        while True:
            if not self.com.recvBuffer.empty():
                temp = self.com.recvBuffer.get()
                if temp == 'Sorting Complete\n':
                    print(temp)
                    break

    def is_achievable(self, point):
        x = point[0]
        y = point[1]
        if x < self.__work_space[0][0] or x > self.__work_space[1][0]:
            return False
        if y < self.__work_space[0][1] or y > self.__work_space[1][1]:
            return False
        if self.__stack_space[0][0] < x < self.__stack_space[1][0] and self.__stack_space[0][1] < y < \
                self.__stack_space[1][1]:
            return False
        return True

    def calibrate_camera_ex_mtx(self):
        points = [(50.0, 0.0), (150.0, 0.0), (250.0, 0.0),
                  (250.0, 100.0), (150.0, 100.0), (50.0, 100.0),
                  (50.0, 200.0), (150.0, 200.0), (250.0, 200.0)]
        offset_x = -90.0
        offset_y = 21.0
        z = -160.0
        flag_points = []
        for point in points:
            flag_point = np.array([point[0] + offset_x, point[1] + offset_y, z])
            flag_points.append(flag_point)
        img_points = []

        self.com.clear_buffer()

        self.com.send_reset()
        self.wait_for_ac()
        print('Reset Complete!')

        for point in points:
            print(point[0], point[1])
            self.com.send_move(point[0], point[1])
            self.wait_for_ac()
            img_point = self.camera.find_flag_point()
            img_point = np.array(img_point)
            img_points.append(img_point)

        self.camera.calibrate_extrinsic_matrix(img_points, flag_points)
        print(self.camera.p2w_mtx)

        self.com.send_move(0.0, 250.0)

    def sort(self, color):
        centers = self.camera.find_objects(color)
        print('原始点', centers)

        # 处理反光导致一个色块识别成多个
        new_centers = []
        adjusted_points = []  # 记录处理过的点
        for center in centers:
            center_in_range = []  # 在范围内的点
            # 如果处理过，则不重复处理
            if is_array_in_list(center, adjusted_points):
                continue
            # 遍历其它点
            for other in centers:
                if other is center:
                    continue
                print('另一点', other)
                print('距离', descartes_distance(center, other))
                if descartes_distance(center, other) < 30.0:
                    center_in_range.append(other)
                    adjusted_points.append(other)
            if center_in_range:
                center_in_range.append(center)
                real_center_array = np.array(center_in_range)
                real_center = real_center_array.mean(axis=0)
                new_centers.append(real_center)
            else:
                new_centers.append(center)

        print(new_centers)
        self.com.clear_buffer()
        for center in new_centers:
            if not self.is_achievable(center):
                continue
            self.com.send_sorting(center[0], center[1], color)
            self.wait_for_bc()
            time.sleep(0.5)
            self.wait_for_cc()
        self.com.send_move(0.0, 250.0)


if __name__ == '__main__':
    Robot = SortingRobot()
    Robot.calibrate_camera_in_mtx()
    Robot.activate_show_camera()
    # hint = "Please enter command: \r\n" \
    #        "CALIBRATE IN MAT \r\n" \
    #        "CALIBRATE EX MAT \r\n"
    # while True:
    #     cmd = input(hint)
    #     if cmd == 'CALIBRATE IN MAT':
    #         Robot.calibrate_camera_in_mtx()
    #     elif cmd == 'CALIBRATE EX MAT':
    #         Robot.calibrate_camera_ex_mtx()
